
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pid_basic.c
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "pid_basic.h"
#include <float.h>
/*-----------------------------------macro------------------------------------*/
#define AC_PID_Basic_FILT_E_HZ_DEFAULT 20.0f   // default input filter frequency
#define AC_PID_Basic_FILT_E_HZ_MIN     0.01f   // minimum input filter frequency
#define AC_PID_Basic_FILT_D_HZ_DEFAULT 10.0f   // default input filter frequency
#define AC_PID_Basic_FILT_D_HZ_MIN     0.005f  // minimum input filter frequency
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Constructor
void pid_basic_ctrl_ctor(PID_basic_ctrl * controler, float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt)
{
    controler->_dt = dt;
    controler->_kp = initial_p;
    controler->_ki = initial_i;
    controler->_kd = initial_d;
    controler->_kff = initial_ff;
    controler->_kimax = fabsf(initial_imax);
    pid_basic_ctrl_set_filt_E_hz(controler, initial_filt_E_hz);
    pid_basic_ctrl_set_filt_D_hz(controler, initial_filt_D_hz);

    // reset input filter to first value received
    controler->_reset_filter = true;
}

float pid_basic_ctrl_update_all(PID_basic_ctrl * controler, float target, float measurement, bool limit)
{
    return pid_basic_ctrl_update_all2(controler, target, measurement, (limit && math_flt_negative(controler->_integrator)), (limit && math_flt_positive(controler->_integrator)));
}

//  update_all - set target and measured inputs to PID controller and calculate outputs
//  target and error are filtered
//  the derivative is then calculated and filtered
//  the integral is then updated based on the setting of the limit flag
float pid_basic_ctrl_update_all2(PID_basic_ctrl * controler, float target, float measurement, bool limit_neg, bool limit_pos)
{
    // don't process inf or NaN
    if (!isfinite(target) || isnan(target) ||
        !isfinite(measurement) || isnan(measurement)) {
        return 0.0f;
    }

    controler->_target = target;

    // reset input filter to value received
    if (controler->_reset_filter) {
        controler->_reset_filter = false;
        controler->_error = controler->_target - measurement;
        controler->_derivative = 0.0f;
    } else {
        float error_last = controler->_error;
        controler->_error += pid_basic_ctrl_get_filt_E_alpha(controler) * ((controler->_target - measurement) - controler->_error);

        // calculate and filter derivative
        if (math_flt_positive(controler->_dt)) {
            float derivative = (controler->_error - error_last) / controler->_dt;
            controler->_derivative += pid_basic_ctrl_get_filt_D_alpha(controler) * (derivative - controler->_derivative);
        }
    }

    // update I term
    pid_basic_ctrl_update_i(controler, limit_neg, limit_pos);

    const float P_out = controler->_error * controler->_kp;
    const float D_out = controler->_derivative * controler->_kd;

    return P_out + controler->_integrator + D_out + controler->_target * controler->_kff;
}

//  update_i - update the integral
//  if limit_neg is true, the integral can only increase
//  if limit_pos is true, the integral can only decrease
void pid_basic_ctrl_update_i(PID_basic_ctrl * controler, bool limit_neg, bool limit_pos)
{
    if (!math_flt_zero(controler->_ki)) {
        // Ensure that integrator can only be reduced if the output is saturated
        if (!((limit_neg && math_flt_negative(controler->_error)) || (limit_pos && math_flt_positive(controler->_error)))) {
            controler->_integrator += ((float)controler->_error * controler->_ki) * controler->_dt;
            controler->_integrator = math_constrain_float(controler->_integrator, -controler->_kimax, controler->_kimax);
        }
    } else {
        controler->_integrator = 0.0f;
    }
}

// get_filt_T_alpha - get the target filter alpha
float pid_basic_ctrl_get_filt_E_alpha(PID_basic_ctrl * controler)
{
    return math_calc_lpf_alpha_dt(controler->_dt, controler->_filt_E_hz);
}

// get_filt_D_alpha - get the derivative filter alpha
float pid_basic_ctrl_get_filt_D_alpha(PID_basic_ctrl * controler)
{
    return math_calc_lpf_alpha_dt(controler->_dt, controler->_filt_D_hz);
}

void pid_basic_ctrl_set_integrator3(PID_basic_ctrl * controler, float target, float measurement, float i)
{
    pid_basic_ctrl_set_integrator2(controler, target - measurement, i);
}

void pid_basic_ctrl_set_integrator2(PID_basic_ctrl * controler, float error, float i)
{
    pid_basic_ctrl_set_integrator(controler, i - error * controler->_kp);
}

void pid_basic_ctrl_set_integrator(PID_basic_ctrl * controler, float i)
{
    controler->_integrator = math_constrain_float(i, -controler->_kimax, controler->_kimax);
}

/*------------------------------------test------------------------------------*/


